#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <string>
#include <time.h>

typedef pcl::PointXYZRGB PointType;
typedef pcl::PointCloud<PointType> kinectPCL;

int getNPoints(kinectPCL::Ptr& cloud) {
	return (int)(cloud->end() - cloud->begin());
}

void downsample(kinectPCL::Ptr& cloud, double factor) {
	srand(time(NULL));//Re-seed random number generator based on time (no seed)
	//so that a new pseudorandom sequence will be generated for sampling
	int NTarget = (int)getNPoints(cloud)*factor;
	while (getNPoints(cloud) > NTarget) {
		int N = getNPoints(cloud);
		int index = rand() % N;
		cloud->points[index] = cloud->points[N-1];
		cloud->points.pop_back();
	}
}

int main(int argc, char** argv) {
    kinectPCL::Ptr cloud(new kinectPCL);
    pcl::io::loadPCDFile(argv[1], *cloud);
    
    printf("NPoints = %i\n", getNPoints(cloud));
    downsample(cloud, 0.001);
    printf("NPointsAfter = %i\n", getNPoints(cloud));
    
	// Normal estimation*
	pcl::NormalEstimation<PointType, pcl::Normal> n;
	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
	pcl::KdTree<PointType>::Ptr tree (new pcl::KdTreeFLANN<PointType>);
	tree->setInputCloud (cloud);
	n.setInputCloud (cloud);
	n.setSearchMethod (tree);
	n.setKSearch (20);
	n.compute (*normals);
	//* normals should not contain the point normals + surface curvatures

	// Concatenate the XYZ and normal fields*
	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
	pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
	//* cloud_with_normals = cloud + normals

	// Create search tree*
	pcl::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>);
	tree2->setInputCloud (cloud_with_normals);

	// Initialize objects
	pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
	pcl::PolygonMesh triangles;

	// Set the maximum distance between connected points (maximum edge length)
	gp3.setSearchRadius (0.025);

	// Set typical values for the parameters
	gp3.setMu (2.5);
	gp3.setMaximumNearestNeighbors (100);
	gp3.setMaximumSurfaceAgle(M_PI/4); // 45 degrees
	gp3.setMinimumAngle(M_PI/18); // 10 degrees
	gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
	gp3.setNormalConsistency(false);

	// Get result
	gp3.setInputCloud (cloud_with_normals);
	gp3.setSearchMethod (tree2);
	gp3.reconstruct (triangles);

	// Additional vertex information
	std::vector<int> parts = gp3.getPartIDs();
	std::vector<int> states = gp3.getPointStates();    
    
    return 0;
}
